/*
 * motors.c
 * Copyright (c) 2013 Maris BECS. All rights reserved.
 */

#include "motors.h"


void motors_init(void){
	// initialize TCD1 timer for servo control
	//pwm_init(&pwm_cfg[0], PWM_TCD1, PWM_CH_A, 50);  // 50 Hz (20ms)
	pwm_init(&pwm_cfg[1], PWM_TCD1, PWM_CH_B, 50);  // 50 Hz (20ms)
	//pwm_start(&pwm_cfg[0], 0);  // right side servo
	pwm_start(&pwm_cfg[1], 0);  // left  side servo

	// initialize TCD0 and TCE0 timers for motor control
	pwm_init(&pwm_cfg[2], PWM_TCD1, PWM_CH_A, 4000);    // 4 kHz    Left  T1   Top pin 0
	pwm_init(&pwm_cfg[3], PWM_TCD0, PWM_CH_D, 4000);    // 4 kHz    Left  T2   Top pin 3
	pwm_init(&pwm_cfg[4], PWM_TCE0, PWM_CH_A, 4000);    // 4 kHz    Right T1   Top pin 0
	pwm_init(&pwm_cfg[5], PWM_TCE0, PWM_CH_D, 4000);    // 4 kHz    Right T2   Top pin 3
	pwm_start(&pwm_cfg[2], 0);  // turn off PWM
	pwm_start(&pwm_cfg[3], 0);  // turn off PWM
	pwm_start(&pwm_cfg[4], 0);  // turn off PWM
	pwm_start(&pwm_cfg[5], 0);  // turn off PWM

	drive(0, 0); // safety first
	stop_motors();
	}


void drive(int8_t left_pwm, int8_t right_pwm){

	#ifdef DEBUG_ENABLE
		usart_serial_putchar( USART_SERIAL, abs(left_pwm) );
		usart_serial_putchar( USART_SERIAL, abs(right_pwm) );
	#endif

	if( 0 == left_pwm ){
		// break left motor
		ioport_set_pin_level(MOTOR_L1, LOW);    // T1 closed
		ioport_set_pin_level(MOTOR_L2, HIGH);   // B1 open
		ioport_set_pin_level(MOTOR_L3, HIGH);   // B2 open
		ioport_set_pin_level(MOTOR_L4, LOW);    // T2 closed
			pwm_start( &pwm_cfg[2], 0 );
			pwm_start( &pwm_cfg[3], 0 );

	}else if( 0 < left_pwm ){
		// drive forward left motor
		pwm_start( &pwm_cfg[2], abs(left_pwm) );	// T1 PWM
		ioport_set_pin_level(MOTOR_L2, LOW);		// B1 closed
		ioport_set_pin_level(MOTOR_L3, HIGH);		// B2 open
		ioport_set_pin_level(MOTOR_L4, LOW);		// T2 closed
			pwm_start( &pwm_cfg[3], 0 );

	}else if( 0 > left_pwm ){
		// drive backward left motor
		ioport_set_pin_level(MOTOR_L1, LOW);		// T1 closed
			pwm_start( &pwm_cfg[2], 0 );
		ioport_set_pin_level(MOTOR_L2, HIGH);		// B1 open
		ioport_set_pin_level(MOTOR_L3, LOW);		// B2 closed
		pwm_start( &pwm_cfg[3], abs(left_pwm) );	// T2 PWM
	}

	if( 0 == right_pwm ){
		// break right motor
		ioport_set_pin_level(MOTOR_R1, LOW);    // T1 closed
		ioport_set_pin_level(MOTOR_R2, HIGH);   // B1 open
		ioport_set_pin_level(MOTOR_R3, HIGH);   // B2 open
		ioport_set_pin_level(MOTOR_R4, LOW);    // T2 closed
			pwm_start( &pwm_cfg[4], 0 );
			pwm_start( &pwm_cfg[5], 0 );

	}else if( 0 > right_pwm ){
		// drive forward right motor
		pwm_start( &pwm_cfg[4], abs(right_pwm) );	// T1 PWM
		ioport_set_pin_level(MOTOR_R2, LOW);		// B1 closed
		ioport_set_pin_level(MOTOR_R3, HIGH);		// B2 open
		ioport_set_pin_level(MOTOR_R4, LOW);		// T2 closed
			pwm_start( &pwm_cfg[5], 0 );

	}else if( 0 < right_pwm ){
		// drive backward right motor
		ioport_set_pin_level(MOTOR_R1, LOW);		// T1 closed
			pwm_start( &pwm_cfg[4], 0 );
		ioport_set_pin_level(MOTOR_R2, HIGH);		// B1 open
		ioport_set_pin_level(MOTOR_R3, LOW);		// B2 closed
		pwm_start( &pwm_cfg[5], abs(right_pwm) );	// T2 PWM
	}
}


void stop_motors(void){
    delay_ms(100);
    // free run
    pwm_start( &pwm_cfg[2], 0 );    // Left  T1
    pwm_start( &pwm_cfg[3], 0 );    // Left  T2
    pwm_start( &pwm_cfg[4], 0 );	// Right T1
    pwm_start( &pwm_cfg[5], 0 );	// Right T2
}


//TODO: rename dir with proper direction i.e. left
void turn_servo(bool dir){
	if( true == dir ){
		//pwm_start( &pwm_cfg[0], 3 );  //  % 0.6ms - min value 3
		pwm_start( &pwm_cfg[1], 7 );    //  ?
	}else{
		//pwm_start( &pwm_cfg[0], 12 ); //  % 2.4ms - max value 12
		pwm_start( &pwm_cfg[1], 4 );    //  ?
	}
}
